#!/usr/bin/env python

import roslib; roslib.load_manifest('bwi_apps')
import rospy

import tf
from tf import Transformer, TransformBroadcaster, TransformListener

from geometry_msgs.msg import PoseWithCovarianceStamped
from nav_msgs.msg import Odometry

class FakeAmcl:
  def __init__(self):
    rospy.init_node('fake_amcl', anonymous=True)

    self.publish_rate = rospy.get_param('~publish_rate', 10)

    self.global_frame = self.resolveTF(rospy.get_param("~global_frame", "/map"))
    self.frame_id = self.resolveTF(rospy.get_param("~frame_id", "map"))
    self.odom_frame_id = self.resolveTF(rospy.get_param("~odom_frame_id", "odom"))

    self.publisher = rospy.Publisher("amcl_pose", PoseWithCovarianceStamped)
    self.odom_subscriber = rospy.Subscriber("odom", Odometry, self.odomCallback)

    self.tf = TransformListener()
    self.lookupTransform()

    rate = rospy.Rate(self.publish_rate)
    while not rospy.is_shutdown():
      self.publishTF()
      try:
        rate.sleep()
      except rospy.exceptions.ROSInterruptException:
        pass

  def resolveTF(self, frame_name):
    transformer = Transformer()
    tf_prefix = transformer.getTFPrefix()

    if len(frame_name) > 0:
      if frame_name[0] == '/':
        return frame_name

    if len(tf_prefix) > 0:
      if tf_prefix[0] == '/':
        return tf_prefix + '/' + frame_name
      else:
        return '/' + tf_prefix + '/' + frame_name
    else:
      return '/' + frame_name

  def publishTF(self):
    
    # publish a static 0 transformation from the global /map in simulation to 
    # the odom of this robot
    br = TransformBroadcaster()
    br.sendTransform((0,0,0),
                     tf.transformations.quaternion_from_euler(0, 0, 0),
                     rospy.Time.now(),
                     self.global_frame,
                     self.odom_frame_id)

  def odomCallback(self, odom):
    pose = PoseWithCovarianceStamped()
    pose.header.frame_id = self.frame_id
    pose.header.stamp = odom.header.stamp
    pose.pose.pose.position.x = odom.pose.pose.position.x - self.translation[0]
    pose.pose.pose.position.y = odom.pose.pose.position.y - self.translation[1]
    pose.pose.pose.position.z = 0
    pose.pose.pose.orientation = odom.pose.pose.orientation
    self.publisher.publish(pose)

  def lookupTransform(self):
    rate = rospy.Rate(10)
    count = 0
    while not rospy.is_shutdown():
      try:
        (trans,rot) = self.tf.lookupTransform(self.global_frame, self.frame_id, rospy.Time(0))
        self.translation = trans
        break
      except tf.Exception as e:
        rate.sleep()
        if count == 10:
          rospy.loginfo("Unable to get the transformation from %s. Trying again..." % self.frame_id)
          count = 0
        count = count + 1

if __name__ == '__main__':
  fake_amcl = FakeAmcl()
